clear all;close all;clc;

%System Data

A10=[1.4 0.1;-0.4 0.2];A20=[1.1 0.2;0.3 0.4];
B10=[0 0.01;0.02 0];B20=[0 0.01;0.02 0];
D10=[-0.7;1];D20=[0.8;-0.5];
C2=[3 0.7];C1=C2;
E1=0.1*eye(2);F1=0.1*eye(2);G1=[0.1;0];
E2=0.2*eye(2);F2=0.2*eye(2);G2=[0.2;0];
e1=0.08;gama=1;
H1=[0.01 0;0 0.01];H2=[0.01 0;0 0.01];
r=1;

n=2;m=1;l=1;
eps=0.08;dM=4;
R1=eye(l);R2=eye(m);
Jdb=0;

%Controller Design

tic
m1=1+eps;   m1=sqrt(m1);
m2=(3*r+1)*(1+eps^-1);   m2=sqrt(m2);
E1bar=e1*(E1); F1bar=e1*(F1); G1bar=e1*(G1);
E2bar=e1*(E2); F2bar=e1*(F2); G2bar=e1*(G2);
alpha=1;

[U1,C10,V1] = svd(C1);
[U2,C20,V2] = svd(C2);

Vp1=V1(1:n,1:l);Vpp1=V1(1:n,(l+1):(n-l+1));
Vp2=V2(1:n,1:l);Vpp2=V2(1:n,(l+1):(n-l+1));

C10=C10(1:l,1:l);
C20=C20(1:l,1:l);

setlmis([]) ;
%Xx1 = lmivar(1,[n 1]);Xx2 = lmivar(1,[n 1]);
X11 = lmivar(1,[l 1]);X12 = lmivar(1,[l 1]);
X21 = lmivar(1,[n-l 1]);X22 = lmivar(1,[n-l 1]);
Xz1 = lmivar(1,[n 1]);Xz2 = lmivar(1,[n 1]);
T1  = lmivar(1,[n 1]);T2  = lmivar(1,[n 1]);
Z11 = lmivar(2,[n l]);Z12 = lmivar(2,[n l]);
Z21 = lmivar(2,[m n]);Z22 = lmivar(2,[m n]);
Z31 = lmivar(2,[n n]);Z32 = lmivar(2,[n n]);
Z41 = lmivar(2,[m l]);Z42 = lmivar(2,[m l]);

%lmiterm([-1 1 1 Xx1],1,1)
%lmiterm([-2 1 1 Xx2],1,1)
lmiterm([-1 1 1 X11],1,1)
lmiterm([-2 1 1 X12],1,1)
lmiterm([-3 1 1 X21],1,1)
lmiterm([-4 1 1 X22],1,1)
lmiterm([-5 1 1 Xz1],1,1)
lmiterm([-6 1 1 Xz2],1,1)
lmiterm([-7 1 1 T1],1,1)
lmiterm([-8 1 1 T2],1,1)

lmiterm([9 1 1 X11],-Vp1,Vp1')
lmiterm([9 1 1 X21],-Vpp1,Vpp1')
lmiterm([9 1 1 T1],dM,1)
lmiterm([9 1 4 X11],m1*Vp1,Vp1'*A10')
lmiterm([9 1 4 X21],m1*Vpp1,Vpp1'*A10')
lmiterm([9 1 4 -Z41],m1*C1',D10')
lmiterm([9 1 5 -Z11],C1',1)
lmiterm([9 1 6 X11],m2*Vp1,Vp1'*E1bar')
lmiterm([9 1 6 X21],m2*Vpp1,Vpp1'*E1bar')
lmiterm([9 1 6 -Z41],m2*C1',G1bar')
lmiterm([9 1 9 X11],gama*m2*Vp1,Vp1'*H1')
lmiterm([9 1 9 X21],gama*m2*Vpp1,Vpp1'*H1')
lmiterm([9 2 2 T1],-1,1)
lmiterm([9 2 4 X11],m1*Vp1,Vp1'*B10')
lmiterm([9 2 4 X21],m1*Vpp1,Vpp1'*B10')
lmiterm([9 2 7 X11],m2*Vp1,Vp1'*F1bar')
lmiterm([9 2 7 X21],m2*Vpp1,Vpp1'*F1bar')
lmiterm([9 3 3 Xz1],-1,1)
lmiterm([9 3 4 -Z21],m1,D10')
lmiterm([9 3 5 -Z31],1,1)
lmiterm([9 3 8 -Z21],m2,G1bar')
lmiterm([9 4 4 X12],-Vp2,Vp2')
lmiterm([9 4 4 X22],-Vpp2,Vpp2')
lmiterm([9 5 5 Xz2],-1,1)
lmiterm([9 6 6 X12],-Vp2,Vp2')
lmiterm([9 6 6 X22],-Vpp2,Vpp2')
lmiterm([9 7 7 X12],-Vp2,Vp2')
lmiterm([9 7 7 X22],-Vpp2,Vpp2')
lmiterm([9 8 8 X12],-Vp2,Vp2')
lmiterm([9 8 8 X22],-Vpp2,Vpp2')
lmiterm([9 9 9 0],-1)

lmiterm([10 1 1 X12],-Vp2,Vp2')
lmiterm([10 1 1 X22],-Vpp2,Vpp2')
lmiterm([10 1 1 T2],dM,1)
lmiterm([10 1 4 X12],m1*Vp2,Vp2'*A20')
lmiterm([10 1 4 X22],m1*Vpp2,Vpp2'*A20')
lmiterm([10 1 4 -Z42],m1*C2',D20')
lmiterm([10 1 5 -Z12],C2',1)
lmiterm([10 1 6 X12],m2*Vp2,Vp2'*E2bar')
lmiterm([10 1 6 X22],m2*Vpp2,Vpp2'*E2bar')
lmiterm([10 1 6 -Z42],m2*C2',G2bar')
lmiterm([10 1 9 X12],gama*m2*Vp2,Vp2'*H2')
lmiterm([10 1 9 X22],gama*m2*Vpp2,Vpp2'*H2')
lmiterm([10 2 2 T2],-1,1)
lmiterm([10 2 4 X12],m1*Vp2,Vp2'*B20')
lmiterm([10 2 4 X22],m1*Vpp2,Vpp2'*B20')
lmiterm([10 2 7 X12],m2*Vp2,Vp2'*F2bar')
lmiterm([10 2 7 X22],m2*Vpp2,Vpp2'*F2bar')
lmiterm([10 3 3 Xz2],-1,1)
lmiterm([10 3 4 -Z22],m1,D20')
lmiterm([10 3 5 -Z32],1,1)
lmiterm([10 3 8 -Z22],m2,G2bar')
lmiterm([10 4 4 X11],-Vp1,Vp1')
lmiterm([10 4 4 X21],-Vpp1,Vpp1')
lmiterm([10 5 5 Xz1],-1,1)
lmiterm([10 6 6 X11],-Vp1,Vp1')
lmiterm([10 6 6 X21],-Vpp1,Vpp1')
lmiterm([10 7 7 X11],-Vp1,Vp1')
lmiterm([10 7 7 X21],-Vpp1,Vpp1')
lmiterm([10 8 8 X11],-Vp1,Vp1')
lmiterm([10 8 8 X21],-Vpp1,Vpp1')
lmiterm([10 9 9 0],-1)

lmiterm([11 1 1 X11],-Vp1,Vp1')
lmiterm([11 1 1 X21],-Vpp1,Vpp1')
lmiterm([11 1 1 T1],dM,1)
lmiterm([11 1 4 X11],m1*Vp1,Vp1'*A10')
lmiterm([11 1 4 X21],m1*Vpp1,Vpp1'*A10')
lmiterm([11 1 4 -Z41],m1*C1',D10')
lmiterm([11 1 5 -Z11],C1',1)
lmiterm([11 1 6 X11],m2*Vp1,Vp1'*E1bar')
lmiterm([11 1 6 X21],m2*Vpp1,Vpp1'*E1bar')
lmiterm([11 1 6 -Z41],m2*C1',G1bar')
lmiterm([11 1 9 X11],gama*m2*Vp1,Vp1'*H1')
lmiterm([11 1 9 X21],gama*m2*Vpp1,Vpp1'*H1')
lmiterm([11 2 2 T1],-1,1)
lmiterm([11 2 4 X11],m1*Vp1,Vp1'*B10')
lmiterm([11 2 4 X21],m1*Vpp1,Vpp1'*B10')
lmiterm([11 2 7 X11],m2*Vp1,Vp1'*F1bar')
lmiterm([11 2 7 X21],m2*Vpp1,Vpp1'*F1bar')
lmiterm([11 3 3 Xz1],-1,1)
lmiterm([11 3 4 -Z21],m1,D10')
lmiterm([11 3 5 -Z31],1,1)
lmiterm([11 3 8 -Z21],m2,G1bar')
lmiterm([11 4 4 X11],-Vp1,Vp1')
lmiterm([11 4 4 X21],-Vpp1,Vpp1')
lmiterm([11 5 5 Xz1],-1,1)
lmiterm([11 6 6 X11],-Vp1,Vp1')
lmiterm([11 6 6 X21],-Vpp1,Vpp1')
lmiterm([11 7 7 X11],-Vp1,Vp1')
lmiterm([11 7 7 X21],-Vpp1,Vpp1')
lmiterm([11 8 8 X11],-Vp1,Vp1')
lmiterm([11 8 8 X21],-Vpp1,Vpp1')
lmiterm([11 9 9 0],-1)

lmiterm([12 1 1 X12],-Vp2,Vp2')
lmiterm([12 1 1 X22],-Vpp2,Vpp2')
lmiterm([12 1 1 T2],dM,1)
lmiterm([12 1 4 X12],m1*Vp2,Vp2'*A20')
lmiterm([12 1 4 X22],m1*Vpp2,Vpp2'*A20')
lmiterm([12 1 4 -Z42],m1*C2',D20')
lmiterm([12 1 5 -Z12],C2',1)
lmiterm([12 1 6 X12],m2*Vp2,Vp2'*E2bar')
lmiterm([12 1 6 X22],m2*Vpp2,Vpp2'*E2bar')
lmiterm([12 1 6 -Z42],m2*C2',G2bar')
lmiterm([12 1 9 X12],gama*m2*Vp2,Vp2'*H2')
lmiterm([12 1 9 X22],gama*m2*Vpp2,Vpp2'*H2')
lmiterm([12 2 2 T2],-1,1)
lmiterm([12 2 4 X12],m1*Vp2,Vp2'*B20')
lmiterm([12 2 4 X22],m1*Vpp2,Vpp2'*B20')
lmiterm([12 2 7 X12],m2*Vp2,Vp2'*F2bar')
lmiterm([12 2 7 X22],m2*Vpp2,Vpp2'*F2bar')
lmiterm([12 3 3 Xz2],-1,1)
lmiterm([12 3 4 -Z22],m1,D20')
lmiterm([12 3 5 -Z32],1,1)
lmiterm([12 3 8 -Z22],m2,G2bar')
lmiterm([12 4 4 X12],-Vp2,Vp2')
lmiterm([12 4 4 X22],-Vpp2,Vpp2')
lmiterm([12 5 5 Xz2],-1,1)
lmiterm([12 6 6 X12],-Vp2,Vp2')
lmiterm([12 6 6 X22],-Vpp2,Vpp2')
lmiterm([12 7 7 X12],-Vp2,Vp2')
lmiterm([12 7 7 X22],-Vpp2,Vpp2')
lmiterm([12 8 8 X12],-Vp2,Vp2')
lmiterm([12 8 8 X22],-Vpp2,Vpp2')
lmiterm([12 9 9 0],-1)

lmiterm([-13 1 1 X11],Vp1,Vp1')
lmiterm([-13 1 1 X21],Vpp1,Vpp1')
lmiterm([-13 1 1 0],-alpha)

lmiterm([-14 1 1 X12],Vp2,Vp2')
lmiterm([-14 1 1 X22],Vpp2,Vpp2')
lmiterm([-14 1 1 0],-alpha)

lmis = getlmis;
[tmin,xfeas] = feasp(lmis);
% Xx1 = dec2mat(lmis,xfeas,Xx1);
% Xx2 = dec2mat(lmis,xfeas,Xx2);
X11 = dec2mat(lmis,xfeas,X11);
X12 = dec2mat(lmis,xfeas,X12);
X21 = dec2mat(lmis,xfeas,X21);
X22 = dec2mat(lmis,xfeas,X22);
Xz1 = dec2mat(lmis,xfeas,Xz1);
Xz2 = dec2mat(lmis,xfeas,Xz2);
T1 = dec2mat(lmis,xfeas,T1);
T2 = dec2mat(lmis,xfeas,T2);
Z11 = dec2mat(lmis,xfeas,Z11);
Z21 = dec2mat(lmis,xfeas,Z21);
Z31 = dec2mat(lmis,xfeas,Z31);
Z12 = dec2mat(lmis,xfeas,Z12);
Z22 = dec2mat(lmis,xfeas,Z22);
Z32 = dec2mat(lmis,xfeas,Z32);
Z41 = dec2mat(lmis,xfeas,Z41);
Z42 = dec2mat(lmis,xfeas,Z42);

% X11=V1'*Xx1*V1;X11=X11(1,1);
% X22=V2'*Xx2*V2;X22=X22(1,1);

Xx1=Vp1*X11*Vp1'+Vpp1*X21*Vpp1';
Xx2=Vp2*X12*Vp2'+Vpp2*X22*Vpp2';

Ad1=Z31*inv(Xz1);Ad2=Z32*inv(Xz2);
Bd1=Z11*U1*C10*inv(X11)*inv(C10)*U1';
Bd2=Z12*U2*C20*inv(X22)*inv(C20)*U2';
Cd1=Z21*inv(Xz1);Cd2=Z22*inv(Xz2);
Dd1=Z41*U1*C10*inv(X11)*inv(C10)*U1';
Dd2=Z42*U2*C20*inv(X12)*inv(C20)*U2';

for i=1:6
    xdb(:,i)=[0.5;-0.5];zdb(:,i)=[0;0];
    ndb(i)=norm(xdb(:,i));
end

T0=dM+2;Tf=T0+40;

%Simulation

for i=T0:Tf
    d=[2*ones((Tf-T0)/4,1);3*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1);2*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1)];
    dp1=e1*sin(i);
    n=mod(i,2);
    if (n>0)
        zdb(:,i) =Ad1*zdb(:,i-1)+Bd1*C1*xdb(:,i-1);
        xdb(:,i)=((A10+dp1*E1)+(D10+dp1*G1)*Dd1*C1)*xdb(:,i-1)+(B10+dp1*F1)*xdb(:,i-1-d(i))+(D10+dp1*G1)*Cd1*zdb(:,i-1)+[0.01*sin(xdb(1,i-1));0];
        udb(:,i-1)=Cd1*zdb(:,i-1)+Dd1*C1*xdb(:,i-1);
        ydb(:,i-1)=C1*xdb(:,i-1);
        ndb(i) =norm(xdb(:,i));
        Vdb(i-1)=xdb(:,i-1)'*inv(Xx1)*xdb(:,i-1);
        Vdb(i-1)=Vdb(i-1)+zdb(:,i-1)'*inv(Xz1)*zdb(:,i-1);
        for k=-1:-1:-dM
            Vdb(i-1)=Vdb(i-1)+xdb(:,i-1+k)'*inv(Xx1)*T1*inv(Xx1)*xdb(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            Vdb(i-1)=Vdb(i-1)+(dM+k)*xdb(:,i-1+k)'*inv(Xx1)*T1*inv(Xx1)*xdb(:,i-1+k);
        end
        Vdb(i-1);
    else
        zdb(:,i) =Ad2*zdb(:,i-1)+Bd2*C2*xdb(:,i-1);
        xdb(:,i)=((A20+dp1*E2)+(D20+dp1*G2)*Dd2*C2)*xdb(:,i-1)+(B20+dp1*F2)*xdb(:,i-1-d(i))+(D20+dp1*G2)*Cd2*zdb(:,i-1)+[0.01*sin(xdb(1,i-1));0];
        udb(:,i-1)=Cd2*zdb(:,i-1)+Dd2*C2*xdb(:,i-1);
        ydb(:,i-1)=C2*xdb(:,i-1);
        ndb(i) =norm(xdb(:,i));
        Vdb(i-1)=xdb(:,i-1)'*inv(Xx2)*xdb(:,i-1);
        Vdb(i-1)=Vdb(i-1)+zdb(:,i-1)'*inv(Xz2)*zdb(:,i-1);
        for k=-1:-1:-dM
            Vdb(i-1)=Vdb(i-1)+xdb(:,i-1+k)'*inv(Xx2)*T2*inv(Xx2)*xdb(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            Vdb(i-1)=Vdb(i-1)+(dM+k)*xdb(:,i-1+k)'*inv(Xx2)*T2*inv(Xx2)*xdb(:,i-1+k);
        end
        Vdb(i-1);
    end  
    Jdb=Jdb+ydb(:,i-1)'*R1*ydb(:,i-1)+udb(:,i-1)'*R2*udb(:,i-1);
end

save('DOF.mat')

CostFunction=Jdb;

udb1max=max(udb(1,T0-1:Tf-1));
udb1min=min(udb(1,T0-1:Tf-1));
udb1Max=max(abs(udb1min),abs(udb1max));

ydb1max=max(ydb(1,T0:Tf-1));
ydb1min=min(ydb(1,T0:Tf-1));
ydb1Max=max(abs(ydb1min),abs(ydb1max));

List={};
Result_table = table(dM,CostFunction,udb1Max,ydb1Max,'RowNames',List)

t=0:Tf-T0;
stairs(t,xdb(:,T0-1:Tf-1)','-','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('States');
figure;stairs(t,udb(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Control Signal (u)');
figure;stairs(t,ydb(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Output (y)');
%figure;stairs(t,Vdb(T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('V');
%figure;plot(t,ndb(T0:Tf)','*');grid on;xlabel('k (sample)');ylabel('Norm of state variables');ylim([0 2.3]);